/**
  ******************************************************************************
  * @file    :MG227.c
  * @author  :MG Team
  * @version :V1.0
  * @date
  * @brief
  ******************************************************************************
***/

/* Includes ------------------------------------------------------------------*/
#include "Includes.h"
#include "math.h"
#include "mg_adv_par_cfg.h"

//standard value
static u32 g_N32 = 11;
static u32 g_N16M = 343750; //@32768hz

//static u8 Xtal_32k_en_flag = 0;

u8 g_adi_sid = 0x00;   //rang[0x00,0x0F]     !!!one may change it
u16 g_adi_did = 0x678; //range[0x00,0xFFF],  !!!one may change it

u8 adva_enabled = 1;
u8 adv_tx_power_enabled = 0;
u8 TxPowerV;

u8 *ble_addr;
u8 ACAD2[] = {0x05, 0x09, 0x6d, 0x67, 0x6d, 0x67};
u8 ACAD2_len = 0;

//u8  gc_adv_data_n_[] = { 0x02, 0x01, 0x06, 0x05, 0x09, 0x6d, 0x67, 0x30, 0x34};

u8 g_pkt_types = 0;

uint8_t temp_buf[4];
uint8_t data_buf[9];
u8 Ptr_info[3];
//BB RC calibration functions
/*
C32' = C32 *  N32 / N16M  * 32000(31250 for 32768)
N32 --- cycle_32k_reg[3:0]   --> 2^4
N16 --- cycle_16m_reg[12:8] cycle_16m_reg[7:0] cycle_frac_reg[0:5]  --> 2^19

For slave role, we can use the simple algorithm(low accuracy)
C32' = C32 *  N32 / N16M'  * 500(31250/64 for 32768)
N16M' --- cycle_16m_reg[12:8] cycle_16m_reg[7:0] --> 2^13
For 2s interval, there has max of 8 32k-clock accuracy.
*/

//causion: here has the accuracy of 500ppm result!!!!

u32 DoRcCal(u32 In /*in rc value*/, unsigned char *DelayFlagBit /*0 or 10us*/) //return calibated rc value
{
    u32 Temp = 0;
    *DelayFlagBit = 0; //default is 0, [xx-10us] ceiling algorithm may be used and round may lead to 0x80 if decimal part(fractional) is less than 10us.

    //if(!Xtal_32k_en_flag)
    {
        //do the rc calibration if any
        //radio_get_RcCalPar(&g_N32,&g_N16M);
        MG_Read_Buffer(RC_CAL_REG, temp_buf, 4);

        g_N32 = temp_buf[1] & 0x1F;

        g_N16M = temp_buf[3];
        g_N16M <<= 8;
        g_N16M |= temp_buf[2];
        g_N16M <<= 6;
        g_N16M |= (temp_buf[0] & 0x3F);

        Temp = In * g_N32 * 500 / ((g_N16M + 0x20) >> 6); //Rc_Cal_Simple(In,g_N32,g_N16M);
    }

    return Temp;
}

void set_adv_interval(void) //set normal adv interval,[g_adv_interval]unit: 1ms
{
    u8 delay_flag;
    u32 interval = DoRcCal(LF_TIME1MS * g_adv_interval, &delay_flag);

#define RC_LOAD 0x01

    MG_Read_Buffer(ADV_INTERV_RC_CFG, data_buf, 4);

    data_buf[0] = interval & 0xff;
    data_buf[1] = (interval >> 8) & 0xff;
    data_buf[2] = (interval >> 16) & 0xff;
    data_buf[3] |= RC_LOAD;

    MG_Write_Buffer(ADV_INTERV_RC_CFG, data_buf, 4);
}

void fill_fifo_data(unsigned char fifo_id /*0:ext,1:chain/sync*/, unsigned char *data, unsigned char len)
{
    MG_Write_Buffer(W_TX_FIFO1 + fifo_id * 2, data, len);
}

//without ACAD part
u8 get_extended_header_len(u8 flag)
{
    u8 i, len = 1;
    const unsigned char HDR_Item_Len[] = {6, 6, 1, 2, 3, 18, 1};

    for (i = 0; i < 7; i++)
    {
        if (flag & (0x01 << i))
            len += HDR_Item_Len[i];
    }
    return len;
}

u8 *get_auxPtr_par(u32 offset /*unit ms*/, u8 ch_idx, u8 phy_id /*0-1Mhz, 2-coded*/)
{
    u8 t;
    u8 CA = 0; //0-:500ppm, 1-:50ppm
    //30~245,700us-30us 2.457s-300us

    t = CA << 6;
    t |= ch_idx;
    if (offset > 245)
    {
        t |= 0x80;
    }
    Ptr_info[0] = t;

    if (offset > 245)
    {
        offset *= 10;
        offset /= 3;
    }
    else
    {
        offset *= 100;
        offset /= 3;
    }
    Ptr_info[1] = offset & 0xff;

    Ptr_info[2] = offset >> 8;
    Ptr_info[2] |= (phy_id << 5);

    return Ptr_info;
}

u8 is_data_updated(void)
{
    u8 tmp;

    MG_Read_Buffer(WR_SYNC_PKT_STATE, &tmp, 1);

    //clear the bit if any(write 0 to clear the bit)
    if (tmp & 0x02)
    {
        tmp ^= 0x02; //clear the bit
        MG_Write_Buffer(WR_SYNC_PKT_STATE, &tmp, 1);
        return 0x02; //fifo update success
    }

    return 0; //fifo update fail
}

void update_adv_ext_ind_data(unsigned char *data, unsigned char len)
{
    u8 hd[5];
    u8 hd_ext[4];
    u16 adv_len;

    MG_Read_Buffer(ADV_PKT_CFG, hd, 5);
    MG_Read_Buffer(EXT_HEADER_CFG1, hd_ext, 4);

    adv_len = (hd_ext[2] & 0x3F) + 1 + len;
    hd[0] = AUX_ADV_IND_BIT; //lock bit
    //hd[0] = DISABLE_LOCK_DATA_BIT;
    hd[4] = adv_len;

    while (1)
    {
        MG_Write_Buffer(ADV_PKT_CFG, hd, 5);
        fill_fifo_data(0, data, len);
        //  return;
        //check reg status
        if (is_data_updated())
            break;
    }
}

void update_aux_chain_ind_data(unsigned char *data, unsigned char len)
{
    u8 hd[7];
    u8 hd_ext[6];
    u16 adv_len;

    MG_Read_Buffer(ADV_PKT_CFG, hd, 7);
    MG_Read_Buffer(EXT_HEADER_CFG1, hd_ext, 6);

    adv_len = (hd_ext[4] & 0x3F) + 1 + len;
    hd[0] = AUX_CHAIN_IND_BIT; //lock bit
    //hd[0] = DISABLE_LOCK_DATA_BIT;
    hd[6] = adv_len;

    while (1)
    {
        MG_Write_Buffer(ADV_PKT_CFG, hd, 7);
        fill_fifo_data(1, data, len);
        //    return;
        //check reg status
        if (is_data_updated())
            break;
    }
}

void update_adv_ext_ind_par(void) //interval,AuxPtr [g_interval_2_aux_step,g_interval_2_aux_step_num]
{
    u8 i;
    u8 delay_flag;
    u32 interval;

    set_adv_interval(); //set/refresh interval RC

    i = 0;
    interval = DoRcCal(g_interval_2_aux_step[i] * LF_TIME1MS, &delay_flag);
    data_buf[0] = interval & 0xff;
    data_buf[1] = (interval >> 8) & 0xff;
    data_buf[2] = (interval >> 16) & 0xff;
    data_buf[3] = (g_interval_2_aux_step_num << 1) | 0x01 | delay_flag;

    if (g_aux_ch_sel == MG_ADV_CH_AUTO)
        data_buf[7] = (AUX_ADV_IND_HOP << 1); //hop
    else
        data_buf[7] = 0;

    if (g_interval_2_aux_step_num > 1)
    {
        interval = DoRcCal(g_interval_2_aux_step[i + 1] * LF_TIME1MS, &delay_flag);
        data_buf[4] = interval & 0xff;
        data_buf[5] = (interval >> 8) & 0xff;
        data_buf[6] = (interval >> 16) & 0xff;
        data_buf[7] |= (0x01 | delay_flag);
    }
    MG_Write_Buffer(OFFSET_RC_CFG1, data_buf, 8);

    if (g_interval_2_aux_step_num <= 2)
        return;

    for (i = 2; i < g_interval_2_aux_step_num; i += 2)
    {
        interval = DoRcCal(g_interval_2_aux_step[i] * LF_TIME1MS, &delay_flag);
        data_buf[0] = interval & 0xff;
        data_buf[1] = (interval >> 8) & 0xff;
        data_buf[2] = (interval >> 16) & 0xff;
        data_buf[3] = 0x01 | delay_flag;

        if (i < g_interval_2_aux_step_num - 1)
        {
            interval = DoRcCal(g_interval_2_aux_step[i + 1] * LF_TIME1MS, &delay_flag);
            data_buf[4] = interval & 0xff;
            data_buf[5] = (interval >> 8) & 0xff;
            data_buf[6] = (interval >> 16) & 0xff;
            data_buf[7] = 0x01 | delay_flag;
        }
        MG_Write_Buffer(OFFSET_RC_CFG1 + (i >> 1), data_buf, 8);
    }
}

void set_adv_ext_ind_par(u8 adv_type, u8 *acad2, u8 acad2_len) //AuxPtr [g_interval_2_aux_step,g_interval_2_aux_step_num]
{
    u8 i;
    u8 flags;
    u8 adv_len;
    u8 hd[6 + 1];
    u8 hd_ext[6];
    u8 tmp[14];

    //#define ADV_EXT_IND_ONLY /*if adv_ext_ind ONLY, open this macro definition to disable aux_adv_ind package.*/

    if (!((g_pkt_types & AUX_SYNC_IND_BIT) || (g_pkt_types & AUX_CHAIN_IND_BIT))) //extended_ind+aux_ind case
    {
        hd[0] = DISABLE_LOCK_DATA_BIT; //unlocked flag

        //first package's info
        flags = (HDR_FLAG_ADI | HDR_FLAG_AUXPTR); //xHDR_FLAG_TXPOWER, xHDR_FLAG_TARGETADDR
//        if(g_ext_phy_sel == MG_ADV_PHY_1M)flags |= HDR_FLAG_ADVA;
#ifdef ADV_EXT_IND_ONLY
        if (g_adv_ext2_type == MG_TYPE_ADV_NOT_USED)
        {
            flags &= ~(HDR_FLAG_AUXPTR | HDR_FLAG_ADI);
            flags |= HDR_FLAG_ADVA;
        }
#endif
        adv_len = get_extended_header_len(flags); //MUST NO ACAD part,NO AdvData  ===> Extended Header Length

        hd_ext[0] = adv_len; //non-connectable non-scanable
        hd_ext[1] = flags;

        adv_len++;
        hd[0 + 1] = adv_type;
        hd[1 + 1] = adv_len;
        hd[4 + 1] = 0x0F;

        //second package's info
        flags = HDR_FLAG_ADI;
        //        if((g_aux_phy_sel == MG_ADV_PHY_500K) || (g_aux_phy_sel == MG_ADV_PHY_125K))
        {
            if (adva_enabled)
                flags |= HDR_FLAG_ADVA; //HDR_FLAG_TXPOWER,HDR_FLAG_TARGETADDR
            if (adv_tx_power_enabled)
                flags |= HDR_FLAG_TXPOWER;
        }
        adv_len = get_extended_header_len(flags); //may ACAD part added if any.  ===> Extended Header Length
        if (acad2_len && acad2)
            adv_len += acad2_len;

        hd_ext[2] = adv_len;
        hd_ext[3] = flags;

        adv_len++;
        adv_len += g_adv_data_pkt2_len;
        hd[2 + 1] = adv_type;
        hd[3 + 1] = adv_len;
#ifdef ADV_EXT_IND_ONLY
        if (g_adv_ext2_type == MG_TYPE_ADV_NOT_USED)
        {
            hd[2 + 1] = 0x0F;
        }
#endif
        MG_Write_Buffer(ADV_PKT_CFG, hd, 5 + 1);     //adv header
        MG_Write_Buffer(EXT_HEADER_CFG1, hd_ext, 4); //extended header

        //acad data if any
        if (acad2_len && acad2)
            MG_Write_Buffer(EXT_HEADER_CFG4, acad2, acad2_len);

        //aux adv ind data
        fill_fifo_data(0, g_adv_data_pkt2, g_adv_data_pkt2_len);
    }

    for (i = 0; i < g_interval_2_aux_step_num; i++)
    {
        u8 *Ptr;
        u8 phy = g_aux_phy_sel;
        u8 _ch = g_aux_ch_sel;

        if ((phy == MG_ADV_PHY_500K) || (phy == MG_ADV_PHY_125K))
            phy = 2;
        else if (phy == MG_ADV_PHY_1M)
            phy = 0;
        else
            phy = 1; //2M
        if (_ch == MG_ADV_CH_AUTO)
            _ch = 0; //start from channel 0, heihei

        Ptr = get_auxPtr_par(g_interval_2_aux_step[i], _ch, phy);
        //set the leading steps value and tuned RC nunbers
        if (i == 0) //first one parameter
        {
            u8 tt[4];
            tt[0] = Ptr[0];
            tt[1] = Ptr[1];
            tt[2] = Ptr[2];
            tt[3] = TxPowerV; //also congfig the tx power value
            //MG_Write_Buffer(EXT_HEADER_CFG3, Ptr,3);
            MG_Write_Buffer(EXT_HEADER_CFG3, tt, 4);
        }
        else
        {
            tmp[2 * i - 2] = Ptr[1];
            tmp[2 * i + 1 - 2] = (Ptr[2] & 0x1F) | (Ptr[0] & 0x80);
        }
    }
    if (g_interval_2_aux_step_num > 1) //write into the regs except the first one(saved in [SYNC_INTERVAL_RC_CFG])
    {
        MG_Write_Buffer(AUX_OFFSET2_8, tmp, (g_interval_2_aux_step_num * 2) - 2);
    }

    update_adv_ext_ind_par(); //update rc(interval included)
}

void set_radio_chain_hop(void)
{
    MG_Read_Buffer(OFFSET_RC_CFG1, data_buf, 8);

    data_buf[8] = 0;
    if (g_chain_ch_sel == MG_ADV_CH_AUTO)
        data_buf[8] = AUX_CHAIN_IND_HOP;

    MG_Write_Buffer(OFFSET_RC_CFG1, data_buf, 9);
}

void update_aux_chain_ind_par(void) //offset rc
{
    u8 delay_flag;
    u32 interval;

    interval = DoRcCal(g_chain_offset * LF_TIME1MS, &delay_flag);
    data_buf[0] = interval & 0xff;
    data_buf[1] = (interval >> 8) & 0xff;
    data_buf[2] = (interval >> 16) & 0xff;
    data_buf[3] = (0x01 | ((delay_flag) ? 0x02 : 0x00)); //bit 1 is now means delay flag bit.....

    MG_Write_Buffer(SYNC_INTERVAL_RC_CFG, data_buf, 4);
}

void set_aux_chain_ind_par(u8 adv_type, u8 *acad2, u8 acad2_len) //[g_chain_offset,g_chain_ch_sel]
{
    u8 flags;
    u8 adv_len;
    u8 hd[6 + 1];
    u8 hd_ext[6];

    u8 phy = g_aux_phy_sel;
    u8 *Ptr;

    hd[0] = DISABLE_LOCK_DATA_BIT; //unlocked flag

    //first pkt
    flags = (HDR_FLAG_ADI | HDR_FLAG_AUXPTR); //xHDR_FLAG_TXPOWER, xHDR_FLAG_TARGETADDR
                                              //    if(g_ext_phy_sel == MG_ADV_PHY_1M)flags |= HDR_FLAG_ADVA;
    adv_len = get_extended_header_len(flags); //MUST NO ACAD part,NO AdvData  ===> Extended Header Length

    hd_ext[0] = adv_len;
    hd_ext[1] = flags;

    adv_len++;
    hd[0 + 1] = adv_type;
    hd[1 + 1] = adv_len;

    //second pkt's info
    flags = HDR_FLAG_ADI | HDR_FLAG_AUXPTR;
    //    if((phy == MG_ADV_PHY_500K) || (phy == MG_ADV_PHY_125K))
    {
        if (adva_enabled)
            flags |= HDR_FLAG_ADVA; //HDR_FLAG_TXPOWER, HDR_FLAG_TARGETADDR
        if (adv_tx_power_enabled)
            flags |= HDR_FLAG_TXPOWER;
    }
    adv_len = get_extended_header_len(flags); //may ACAD part added if any.  ===> Extended Header Length
    if (acad2_len && acad2)
        adv_len += acad2_len;

    hd_ext[2] = adv_len;
    hd_ext[3] = flags;

    adv_len++;
    adv_len += g_adv_data_pkt2_len;
    hd[2 + 1] = adv_type;
    hd[3 + 1] = adv_len;

    //third pkt, chain
    flags = HDR_FLAG_ADI; //HDR_FLAG_TXPOWER
    if (adv_tx_power_enabled)
        flags |= HDR_FLAG_TXPOWER;
    adv_len = get_extended_header_len(flags); //no ACAD part.  ===> Extended Header Length
    hd_ext[4] = adv_len;
    hd_ext[5] = flags;

    adv_len++;
    adv_len += g_adv_data_pkt3_len;
    hd[4 + 1] = adv_type;
    hd[5 + 1] = adv_len;

    MG_Write_Buffer(ADV_PKT_CFG, hd, 6 + 1);     //adv header
    MG_Write_Buffer(EXT_HEADER_CFG1, hd_ext, 6); //extended header

    //acad data if any
    if (acad2_len && acad2)
        MG_Write_Buffer(EXT_HEADER_CFG4, acad2, acad2_len);

    //aux adv ind data
    if (g_adv_data_pkt2_len)
        fill_fifo_data(0, g_adv_data_pkt2, g_adv_data_pkt2_len);
    //aux chain ind data
    if (g_adv_data_pkt3_len)
        fill_fifo_data(1, g_adv_data_pkt3, g_adv_data_pkt3_len);

    set_adv_ext_ind_par(adv_type, acad2, acad2_len); //generate Extened_ind's Ptr(s),//step values, to be added

    //aux_adv_ind-->ptr
    if ((phy == MG_ADV_PHY_500K) || (phy == MG_ADV_PHY_125K))
        phy = 2;
    else if (phy == MG_ADV_PHY_2M)
        phy = 1;
    else
        phy = 0; //1M
    Ptr = get_auxPtr_par(g_chain_offset, g_chain_ch_sel == MG_ADV_CH_AUTO ? 0 : g_chain_ch_sel, phy);

    //chain ptr
    MG_Read_Buffer(EXT_HEADER_CFG3, data_buf, 3); //load the aux_ptr-1 values
    data_buf[3] = TxPowerV;
    data_buf[4] = Ptr[0];
    data_buf[5] = Ptr[1];
    data_buf[6] = Ptr[2];
    data_buf[7] = TxPowerV;
    data_buf[8] = TxPowerV;
    MG_Write_Buffer(EXT_HEADER_CFG3, data_buf, 9);

    set_radio_chain_hop();

    update_aux_chain_ind_par(); //update rc
}

u8 ACAD3[10] = {0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x99, 0xaa};
u8 ACAD3_len = 4;
const u8 g_sync_map[5] = {0xff, 0xff, 0xff, 0xff, 0x1f};
const u8 g_sync_crc_init[3] = {0xA5, 0x69, 0x36};
u8 g_sync_aa[4] = {0x12, 0x34, 0x56, 0x78}; //!!!one may random it
u8 g_syncInfo[18];

void set_sync_ind_offset(u32 offset /*ms*/) //!!! It MUST always less than 2.457s ==> Offset Adjust = 0 !!!
{
    u8 unit_value = 0;

    //30~245,700us-30us 2.457s-300us
    if (offset > 245)
    {
        unit_value = 0x20;

        offset *= 10;
        offset /= 3;
    }
    else
    {
        offset *= 100;
        offset /= 3;
    }

    g_syncInfo[0] = offset & 0xff;
    g_syncInfo[1] = offset >> 8;
    g_syncInfo[1] |= unit_value;
}

void set_sync_ind_interval(u16 interval /*ms*/)
{
    //unit change to 1.25ms
    interval *= 4;
    interval /= 5;

    g_syncInfo[2] = interval & 0xff;
    g_syncInfo[3] = interval >> 8;
}

void set_secondary_adv_channel_map(u8 *map /*5Bytes*/)
{
    g_syncInfo[4] = map[0];
    g_syncInfo[5] = map[1];
    g_syncInfo[6] = map[2];
    g_syncInfo[7] = map[3];
    g_syncInfo[8] = (map[4] & 0x1F); // | (g_syncInfo[8] & 0xE0);

    //sca = 0: 500ppm ??
}

void set_sync_ind_aa(u8 *aa)
{
    g_syncInfo[9] = aa[0];
    g_syncInfo[10] = aa[1];
    g_syncInfo[11] = aa[2];
    g_syncInfo[12] = aa[3];
}

void set_sync_ind_crc_init(u8 *sync_crc_init)
{
    g_syncInfo[13] = sync_crc_init[0];
    g_syncInfo[14] = sync_crc_init[1];
    g_syncInfo[15] = sync_crc_init[2];
}

void set_sync_ind_counter(u16 counter)
{
    g_syncInfo[16] = counter & 0xFF;
    g_syncInfo[17] = counter >> 8;
}

void set_sync_ind_info(void)
{
    u8 ch = g_aux_ch_sel;
    u8 phy = g_aux_phy_sel;
    u8 *Ptr;

    if ((phy == MG_ADV_PHY_125K) || (phy == MG_ADV_PHY_500K))
        phy = 2;
    else if (phy == MG_ADV_PHY_2M)
        phy = 1;
    else
        phy = 0; //1M
    if (ch == MG_ADV_CH_AUTO)
        ch = 0; //start from channel 0

    //generate sync_info
    set_sync_ind_aa(g_sync_aa);
    set_sync_ind_crc_init((u8 *)g_sync_crc_init);
    set_secondary_adv_channel_map((u8 *)g_sync_map);
    set_sync_ind_counter(0x64);
    set_sync_ind_interval(g_adv_interval);
    set_sync_ind_offset(g_sync_offset2_2_sync);

    MG_Write_Buffer(SYNC_INFO_CFG1, g_syncInfo, 16);

    data_buf[2] = g_syncInfo[16];
    data_buf[3] = g_syncInfo[17];
    data_buf[4] = 0x01; //reload
    MG_Write_Buffer(SYNC_INFO_CFG2, data_buf, 5);

    //generate aux_ind Ptr info
    Ptr = get_auxPtr_par(g_sync_offset1_2_aux, ch, phy);
    data_buf[0] = Ptr[0];
    data_buf[1] = Ptr[1];
    data_buf[2] = Ptr[2];
    data_buf[3] = TxPowerV;
    //no ptr for aux_sync_ind(using SynInfo @ set_sync_ind_offset(...))
    data_buf[7] = data_buf[8] = data_buf[3];
    MG_Write_Buffer(EXT_HEADER_CFG3, data_buf, 9);
}

void update_aux_sync_ind_par(void) //set rc vaule
{
    u8 delay_flag;
    u32 interval;

    //g_adv_interval ==> sync interval
    interval = DoRcCal(g_adv_interval * LF_TIME1MS, &delay_flag);
    data_buf[0] = interval & 0xff;
    data_buf[1] = (interval >> 8) & 0xff;
    data_buf[2] = (interval >> 16) & 0xff;
    data_buf[3] = (g_interval_2_sync_step_num << 4) | 0x01;
    MG_Write_Buffer(SYNC_INTERVAL_RC_CFG, data_buf, 4);

    //g_sync_offset1_2_aux,
    //g_sync_offset2_2_sync
    interval = DoRcCal(g_sync_offset1_2_aux * LF_TIME1MS, &delay_flag);
    data_buf[0] = interval & 0xff;
    data_buf[1] = (interval >> 8) & 0xff;
    data_buf[2] = (interval >> 16) & 0xff;
    data_buf[3] = 0x01 | delay_flag;
    interval = DoRcCal(g_sync_offset2_2_sync * LF_TIME1MS, &delay_flag);
    data_buf[4] = interval & 0xff;
    data_buf[5] = (interval >> 8) & 0xff;
    data_buf[6] = (interval >> 16) & 0xff;
    data_buf[7] = 0x01 | delay_flag;
    if (g_aux_ch_sel == MG_ADV_CH_AUTO)
        data_buf[7] |= (AUX_ADV_IND_HOP << 1); //hop
    MG_Write_Buffer(OFFSET_RC_CFG1, data_buf, 8);
}

void set_aux_sync_ind_par(u8 adv_type, u8 *acad2, u8 acad2_len, u8 *acad3, u8 acad3_len) //[aa,g_adv_interval,g_interval_2_sync_step_num,g_sync_offset1_2_aux,g_sync_offset2_2_sync]
{
    u8 flags;
    u8 adv_len;
    u8 hd[6 + 1];
    u8 hd_ext[6];

    hd[0] = DISABLE_LOCK_DATA_BIT; //unlocked flag

    //extended adv ind
    flags = (HDR_FLAG_ADI | HDR_FLAG_AUXPTR | HDR_FLAG_TXPOWER); //xHDR_FLAG_TXPOWER, xHDR_FLAG_TARGETADDR
                                                                 //    if(g_ext_phy_sel == MG_ADV_PHY_1M)flags |= HDR_FLAG_ADVA;
    adv_len = get_extended_header_len(flags);                    //MUST NO ACAD part,NO AdvData   ===> Extended Header Length
    hd_ext[0] = adv_len;
    hd_ext[1] = flags;

    adv_len++;
    hd[0 + 1] = adv_type;
    hd[1 + 1] = adv_len;

    //aux adv ind
    flags = HDR_FLAG_ADI | HDR_FLAG_SYNCINFO | HDR_FLAG_TXPOWER;
    //    if((g_aux_phy_sel == MG_ADV_PHY_500K) || (g_aux_phy_sel == MG_ADV_PHY_125K))
    {
        if (adva_enabled)
            flags |= HDR_FLAG_ADVA; //HDR_FLAG_TXPOWER, HDR_FLAG_TARGETADDR
        if (adv_tx_power_enabled)
            flags |= HDR_FLAG_TXPOWER;
    }
    adv_len = get_extended_header_len(flags); //may ACAD part added if any.   ===> Extended Header Length
    if (acad2_len && acad2)
        adv_len += acad2_len;
    hd_ext[2] = adv_len;
    hd_ext[3] = flags;

    adv_len++;
    adv_len += g_adv_data_pkt2_len;
    hd[2 + 1] = adv_type;
    hd[3 + 1] = adv_len;

    //aux sync ind
    flags = 0; // | HDR_FLAG_TXPOWER
    if (adv_tx_power_enabled)
        flags |= HDR_FLAG_TXPOWER;
    if (g_aoa_enable_flag)
        flags |= HDR_FLAG_CTEINFO;            //HDR_FLAG_TXPOWER
    adv_len = get_extended_header_len(flags); //may ACAD part added if any.  ===> Extended Header Length
    if (acad3_len && acad3)
        adv_len += acad3_len;
    hd_ext[4] = adv_len;
    hd_ext[5] = flags;

    adv_len++;
    adv_len += g_adv_data_pkt3_len;
    hd[4 + 1] = adv_type;
    hd[5 + 1] = adv_len;

    MG_Write_Buffer(ADV_PKT_CFG, hd, 7);         //adv header
    MG_Write_Buffer(EXT_HEADER_CFG1, hd_ext, 6); //extended header

    //acad data if any
    if (acad2_len && acad2)
        MG_Write_Buffer(EXT_HEADER_CFG4, acad2, acad2_len);
    if (acad3_len && acad3)
        MG_Write_Buffer(EXT_HEADER_CFG5, acad3, acad3_len);

    //aux adv ind data
    if (g_adv_data_pkt2_len)
        fill_fifo_data(0, g_adv_data_pkt2, g_adv_data_pkt2_len);
    //aux chain ind data
    if (g_adv_data_pkt3_len)
        fill_fifo_data(1, g_adv_data_pkt3, g_adv_data_pkt3_len);

    set_sync_ind_info(); //ptr,aa, and sync_counter,...

    //parameter
    //g_adv_interval ==> sync interval
    //g_interval_2_sync_step_num,
    //g_sync_offset1_2_aux,
    //g_sync_offset2_2_sync
    update_aux_sync_ind_par(); //update rc
}

void set_radio_phy(void)
{
    u8 tmp;

    MG_Read_Buffer(BASEBAND_CFG, &tmp, 1);

    tmp &= 0x07;
    if ((g_aux_phy_sel == MG_ADV_PHY_500K) || (g_aux_phy_sel == MG_ADV_PHY_125K))
    {
        tmp |= (0x01 << 5);

        if (MG_ADV_PHY_500K == g_aux_phy_sel)
            tmp |= (0x01 << 3);
    }
    else
    {
        //need do nothing(must be in 1M phy condition), but for BQB tes mode(2M case), one must cfg the related reg [cont_test_2m].
    }

    MG_Write_Buffer(BASEBAND_CFG, &tmp, 1);
}
///
void set_primary_adv_channel_map(u8 map) /*MAP_CHANNEL_xx_BIT*/
{
    u8 tmp[4];

    map ^= 0xE0; //change to mask bit value

    MG_Read_Buffer(ADV_INTERV_RC_CFG, tmp, 4);
    tmp[3] &= 0x1E;
    tmp[3] |= (map);

    MG_Write_Buffer(ADV_INTERV_RC_CFG, tmp, 4);
}

void set_ADI_AoA_par(void)
{
    u8 adi_info[2];
    //   u8 cte_info; //AoA Constant Tone Extension
    u8 tmp[7];

    //if(g_adv_type < MG_TYPE_ADV_EXT_IND)return;//normal adv

    //cte_info = g_cte_length | (0x00 << 6); //AoA Constant Tone Extension

    adi_info[0] = g_adi_did & 0xFF;
    adi_info[1] = (g_adi_did >> 8) | (g_adi_sid << 4);

    tmp[0] = adi_info[0];
    tmp[1] = adi_info[1];
    tmp[2] = adi_info[0];
    tmp[3] = adi_info[1];
    tmp[4] = adi_info[0];
    tmp[5] = adi_info[1];
    tmp[6] = 0; //no aoa

    MG_Write_Buffer(EXT_HEADER_CFG2, tmp, 7);
}
///
void radio_setIrqMask(u8 mask)
{
    MG_Write_Buffer(INT_MASK, &mask, 1);
}
///
void radio_clearIRQ(u8 irq_status)
{
    MG_Write_Buffer(INT_FLAG, &irq_status, 1);
}

u8 radio_getIrqState(void)
{
    u8 tmp;

    MG_Read_Buffer(INT_FLAG, &tmp, 1);

    if (tmp)
        MG_Write_Buffer(INT_FLAG, &tmp, 1);

    return tmp;
}

/*******************************************************************************
* Function   :     	BLE_TX_Ext
* Parameter  :     	
* Returns    :     	void
* Description:
* Note:      :
*******************************************************************************/
void BLE_TX_Ext(void)
{
    //radio_setIrqMask(0x6a);
    //BLE_Mode_PwrUp();

    set_radio_phy();
    //set_primary_adv_channel_map(MAP_CHANNEL_37_BIT|MAP_CHANNEL_38_BIT|MAP_CHANNEL_39_BIT);
    set_ADI_AoA_par();

    //adv ext ind + aux adv ind + aux chain
    g_pkt_types = ADV_EXTEND_IND_BIT;

    switch (g_adv_ext3_type)
    {
    case MG_TYPE_ADV_NOT_USED: //adv ext ind + aux adv ind case(with big size of adv data)
        g_pkt_types |= AUX_ADV_IND_BIT;
        set_adv_ext_ind_par(TYPE_ADV_EXT_IND | TX_ADDR_TYPE, ACAD2, ACAD2_len);
        break;
    case MG_TYPE_AUX_CHAIN_IND: //adv ext ind + aux adv ind + aux chain ind case
        g_pkt_types |= (AUX_ADV_IND_BIT | AUX_CHAIN_IND_BIT);
        set_aux_chain_ind_par(TYPE_ADV_EXT_IND | TX_ADDR_TYPE, ACAD2, ACAD2_len);
        break;
    case MG_TYPE_AUX_SYNC_IND: //adv ext ind + aux adv ind + aux sync ind(CTE) case
        g_pkt_types |= (AUX_ADV_IND_BIT | AUX_SYNC_IND_BIT);
        set_aux_sync_ind_par(TYPE_ADV_EXT_IND | TX_ADDR_TYPE, ACAD2, ACAD2_len, ACAD3, ACAD3_len);
        break;
    default: //error
        //while(1);
        break;
    }

    //clear all interrupt
    //data_buf[0] = 0xFF;
    //MG_Write_Buffer(INT_FLAG, data_buf, 1);

    MG_Write_Reg(BASEBAND_CFG, 5); //SLP_XO=4, STOP=2, WKUP_EN=1
    MG_Write_Reg(WAKEUP_NOW, 1);
}
